#include "mainwindow.h"
#include "ui_mainwindow.h"
#include <QPainter>
#include <QImage>
#include <QDebug>
#include <QSettings>
#include <QMessageBox>
#include <opencv2/core/core.hpp>
#include <opencv2/opencv.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <QTimer>
#include <QDateTime>
#include <QString>

MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
    QMainWindow(parent),
    ui(new Ui::MainWindow),
    qRosNode(argc, argv)
{
    memset(bControlBtnState, false, sizeof(bool) * 6);
    ui->setupUi(this);
    ReadSettings();
    setWindowIcon(QIcon(":/image/images/NoCamera.png"));

    ui->tabWidget->setCurrentIndex(0);
    ui->button_break->setEnabled(false);
    ui->listView_log->setModel(qRosNode.logModel());

    /*rosnode -> mainwindow*/
    qRegisterMetaType< QVector<int> >("QVector<int>");
    QObject::connect(&qRosNode, SIGNAL(signalLogUpdated()), this, SLOT(slotLogViewUpdate()));
    qRegisterMetaType< cv::Mat >("cv::Mat");
    connect(&qRosNode, SIGNAL(signalMatUpdate(const cv::Mat &)), this, SLOT(slotMatUpdate(const cv::Mat &)));
    connect(&qRosNode, SIGNAL(signalRosTriggerUiUpdate()), this, SLOT(slotRosTriggerUiUpdate()));

    connect(&timer,SIGNAL(timeout()),this,SLOT(handleTimeOut()));

    /*mainwindow -> rosnode*/
    QObject::connect(this, SIGNAL(signalRosShutdown()), &qRosNode, SLOT(slotRosClose()));
    QObject::connect(this, SIGNAL(signalRosActionTrigger()), &qRosNode, SLOT(slotRosActionHandle()));

    QVBoxLayout *layout = new QVBoxLayout();
    layout->addWidget(&imageShow);
    ui->tab_rosView->setLayout(layout);
    ui->groupBox_targetGps->hide();
    ui->button_quit->setStyleSheet("background-color: rgb(175,238,238)");
}

MainWindow::~MainWindow()
{
    delete ui;
}

void MainWindow::showNoMasterMessage() {
    QMessageBox msgBox;
    msgBox.setText("Couldn't find the ros master.");
    msgBox.exec();
    close();
}

void MainWindow::on_button_connect_clicked()
{
    if ( ui->checkBox_use_default_param->isChecked() ) {
        if ( !qRosNode.init() ) {
            showNoMasterMessage();
        } else {
            ui->button_break->setEnabled(true);
            ui->button_connect->setEnabled(false);
            ui->checkBox_use_default_param->setEnabled(false);
        }
    } else {
        if ( ! qRosNode.init(ui->lineEdit_master_url->text().toStdString(),
                   ui->lineEdit_ros_ip->text().toStdString()) ) {
            showNoMasterMessage();
        } else {
            ui->button_break->setEnabled(true);
            ui->button_connect->setEnabled(false);
            ui->lineEdit_master_url->setReadOnly(true);
            ui->lineEdit_ros_ip->setReadOnly(true);
            ui->checkBox_use_default_param->setEnabled(false);
        }
    }
}

void MainWindow::on_button_break_clicked()
{
    ui->button_break->setEnabled(false);
    ui->button_connect->setEnabled(true);
    ui->lineEdit_master_url->setReadOnly(false);
    ui->lineEdit_ros_ip->setReadOnly(false);
    Q_EMIT signalRosShutdown();
}

void MainWindow::slotMatUpdate(const cv::Mat &inMat)
{
    if(!inMat.empty())
    {
        cv::Mat destMat;
        cv::cvtColor(inMat,destMat,cv::COLOR_BGR2RGB);
        QImage image = QImage((const unsigned char*)(destMat.data), destMat.cols, destMat.rows, destMat.step, QImage::Format_RGB888).copy();
        QImage img = image.scaled(ui->tab_rosView->width(),ui->tab_rosView->height());
        imageShow.setImage(img);
        imageShow.update();
    }

    //qRosNode.s_bIsMatUpdateFinished = true;
}

/*
void MainWindow::slotRosTriggerUiUpdate()
{
//    ui->lineEdit_longitude->setText(QString::number(qRosNode.s_stDecodeMsg.aGlobalPos[0],'f',7));
//    ui->lineEdit_latitude->setText(QString::number(qRosNode.s_stDecodeMsg.aGlobalPos[1],'f',7));
//    ui->lineEdit_altitude->setText(QString::number(qRosNode.s_stDecodeMsg.aGlobalPos[2],'f',7));
    Cluster_Context *pstClusterCntxt = fnCluster_ctrl_GetClusterCntxt();
    ui->lineEdit_mainNodeID->setText(QString::number(pstClusterCntxt->uMainNodeID,'f',7));
    ui->lineEdit_subNodeNum->setText(QString::number(pstClusterCntxt->uSubNodeNum,'f',7));

    ui->lineEdit_uavState_1->setText(QString::number(pstClusterCntxt->astSubNodeState[1].enClusterNetworkState,'f',7));
    ui->lineEdit_uavState_2->setText(QString::number(pstClusterCntxt->astSubNodeState[2].enClusterNetworkState,'f',7));
    ui->lineEdit_uavState_3->setText(QString::number(pstClusterCntxt->astSubNodeState[3].enClusterNetworkState,'f',7));
    ui->lineEdit_uavState_4->setText(QString::number(pstClusterCntxt->astSubNodeState[4].enClusterNetworkState,'f',7));
    ui->lineEdit_uavState_5->setText(QString::number(pstClusterCntxt->astSubNodeState[5].enClusterNetworkState,'f',7));

    ui->lineEdit_subNodeState_1->setText(QString::number(pstClusterCntxt->astSubNodeState[1].enClusterSubNodeState,'f',7));
    ui->lineEdit_subNodeState_2->setText(QString::number(pstClusterCntxt->astSubNodeState[2].enClusterSubNodeState,'f',7));
    ui->lineEdit_subNodeState_3->setText(QString::number(pstClusterCntxt->astSubNodeState[3].enClusterSubNodeState,'f',7));
    ui->lineEdit_subNodeState_4->setText(QString::number(pstClusterCntxt->astSubNodeState[4].enClusterSubNodeState,'f',7));
    ui->lineEdit_subNodeState_5->setText(QString::number(pstClusterCntxt->astSubNodeState[5].enClusterSubNodeState,'f',7));

    ui->lineEdit_missionState_1->setText(QString::number(pstClusterCntxt->astSubNodeState[1].enClusterMissionState,'f',7));
    ui->lineEdit_missionState_2->setText(QString::number(pstClusterCntxt->astSubNodeState[2].enClusterMissionState,'f',7));
    ui->lineEdit_missionState_3->setText(QString::number(pstClusterCntxt->astSubNodeState[3].enClusterMissionState,'f',7));
    ui->lineEdit_missionState_4->setText(QString::number(pstClusterCntxt->astSubNodeState[4].enClusterMissionState,'f',7));
    ui->lineEdit_missionState_5->setText(QString::number(pstClusterCntxt->astSubNodeState[5].enClusterMissionState,'f',7));

    ui->lineEdit_workState_1->setText(QString::number(pstClusterCntxt->astSubNodeState[1].enClusterWorkState,'f',7));
    ui->lineEdit_workState_2->setText(QString::number(pstClusterCntxt->astSubNodeState[2].enClusterWorkState,'f',7));
    ui->lineEdit_workState_3->setText(QString::number(pstClusterCntxt->astSubNodeState[3].enClusterWorkState,'f',7));
    ui->lineEdit_workState_4->setText(QString::number(pstClusterCntxt->astSubNodeState[4].enClusterWorkState,'f',7));
    ui->lineEdit_workState_5->setText(QString::number(pstClusterCntxt->astSubNodeState[5].enClusterWorkState,'f',7));

    //qRosNode.s_bIsUiUpdateFinished = true;
}
*/

void MainWindow::slotRosTriggerUiUpdate()
{
    Cluster_Context *pstClusterCntxt = fnCluster_ctrl_GetClusterCntxt();
    ui->lineEdit_mainNodeID->setText(QString::number(pstClusterCntxt->uMainNodeID,'f',7));
    ui->lineEdit_subNodeNum->setText(QString::number(pstClusterCntxt->uSubNodeNum,'f',7));
    //RosNode::s_LogStr.append(PRINT_HEADER+QString::asprintf("uLongitude[%lf], [%lf]",qRosNode.stQgcSaveMsg.stUavInfo[0].uLongitude, qRosNode.stQgcSaveMsg.stUavInfo[0].uLongitude));
    /*ui->lineEdit_uavState_1->setText(QString::number(qRosNode.stQgcSaveMsg.stUavInfo[0].uHwState,'f',7));
    ui->lineEdit_uavState_2->setText(QString::number(qRosNode.stQgcSaveMsg.stUavInfo[1].uHwState,'f',7));
    ui->lineEdit_uavState_3->setText(QString::number(qRosNode.stQgcSaveMsg.stUavInfo[2].uHwState,'f',7));
    ui->lineEdit_uavState_4->setText(QString::number(qRosNode.stQgcSaveMsg.stUavInfo[3].uHwState,'f',7));
    ui->lineEdit_uavState_5->setText(QString::number(qRosNode.stQgcSaveMsg.stUavInfo[4].uHwState,'f',7));
    ui->lineEdit_uavState_6->setText(QString::number(qRosNode.stQgcSaveMsg.stUavInfo[5].uHwState,'f',7));
    ui->lineEdit_uavState_7->setText(QString::number(qRosNode.stQgcSaveMsg.stUavInfo[6].uHwState,'f',7));
    ui->lineEdit_uavState_8->setText(QString::number(qRosNode.stQgcSaveMsg.stUavInfo[7].uHwState,'f',7));
    ui->lineEdit_uavState_9->setText(QString::number(qRosNode.stQgcSaveMsg.stUavInfo[8].uHwState,'f',7));
    ui->lineEdit_uavState_10->setText(QString::number(qRosNode.stQgcSaveMsg.stUavInfo[9].uHwState,'f',7));*/

    ui->lineEdit_uavState_1->setText(QString::number(qRosNode.stQgcSaveMsg.stUavInfo[0].uHwState));
    ui->lineEdit_uavState_2->setText(QString::number(qRosNode.stQgcSaveMsg.stUavInfo[1].uHwState));
    ui->lineEdit_uavState_3->setText(QString::number(qRosNode.stQgcSaveMsg.stUavInfo[2].uHwState));
    ui->lineEdit_uavState_4->setText(QString::number(qRosNode.stQgcSaveMsg.stUavInfo[3].uHwState));
    ui->lineEdit_uavState_5->setText(QString::number(qRosNode.stQgcSaveMsg.stUavInfo[4].uHwState));
    ui->lineEdit_uavState_6->setText(QString::number(qRosNode.stQgcSaveMsg.stUavInfo[5].uHwState));
    ui->lineEdit_uavState_7->setText(QString::number(qRosNode.stQgcSaveMsg.stUavInfo[6].uHwState));
    ui->lineEdit_uavState_8->setText(QString::number(qRosNode.stQgcSaveMsg.stUavInfo[7].uHwState));
    ui->lineEdit_uavState_9->setText(QString::number(qRosNode.stQgcSaveMsg.stUavInfo[8].uHwState));
    ui->lineEdit_uavState_10->setText(QString::number(qRosNode.stQgcSaveMsg.stUavInfo[9].uHwState));

    ui->lineEdit_longitude_1->setText(QString::number(qRosNode.stQgcSaveMsg.stUavInfo[0].uLongitude,'f',7));
    ui->lineEdit_longitude_2->setText(QString::number(qRosNode.stQgcSaveMsg.stUavInfo[1].uLongitude,'f',7));
    ui->lineEdit_longitude_3->setText(QString::number(qRosNode.stQgcSaveMsg.stUavInfo[2].uLongitude,'f',7));
    ui->lineEdit_longitude_4->setText(QString::number(qRosNode.stQgcSaveMsg.stUavInfo[3].uLongitude,'f',7));
    ui->lineEdit_longitude_5->setText(QString::number(qRosNode.stQgcSaveMsg.stUavInfo[4].uLongitude,'f',7));
    ui->lineEdit_longitude_6->setText(QString::number(qRosNode.stQgcSaveMsg.stUavInfo[5].uLongitude,'f',7));
    ui->lineEdit_longitude_7->setText(QString::number(qRosNode.stQgcSaveMsg.stUavInfo[6].uLongitude,'f',7));
    ui->lineEdit_longitude_8->setText(QString::number(qRosNode.stQgcSaveMsg.stUavInfo[7].uLongitude,'f',7));
    ui->lineEdit_longitude_9->setText(QString::number(qRosNode.stQgcSaveMsg.stUavInfo[8].uLongitude,'f',7));
    ui->lineEdit_longitude_10->setText(QString::number(qRosNode.stQgcSaveMsg.stUavInfo[9].uLongitude,'f',7));


    ui->lineEdit_latitude_1->setText(QString::number(qRosNode.stQgcSaveMsg.stUavInfo[0].uLatitude,'f',7));
    ui->lineEdit_latitude_2->setText(QString::number(qRosNode.stQgcSaveMsg.stUavInfo[1].uLatitude,'f',7));
    ui->lineEdit_latitude_3->setText(QString::number(qRosNode.stQgcSaveMsg.stUavInfo[2].uLatitude,'f',7));
    ui->lineEdit_latitude_4->setText(QString::number(qRosNode.stQgcSaveMsg.stUavInfo[3].uLatitude,'f',7));
    ui->lineEdit_latitude_5->setText(QString::number(qRosNode.stQgcSaveMsg.stUavInfo[4].uLatitude,'f',7));
    ui->lineEdit_latitude_6->setText(QString::number(qRosNode.stQgcSaveMsg.stUavInfo[5].uLatitude,'f',7));
    ui->lineEdit_latitude_7->setText(QString::number(qRosNode.stQgcSaveMsg.stUavInfo[6].uLatitude,'f',7));
    ui->lineEdit_latitude_8->setText(QString::number(qRosNode.stQgcSaveMsg.stUavInfo[7].uLatitude,'f',7));
    ui->lineEdit_latitude_9->setText(QString::number(qRosNode.stQgcSaveMsg.stUavInfo[8].uLatitude,'f',7));
    ui->lineEdit_latitude_10->setText(QString::number(qRosNode.stQgcSaveMsg.stUavInfo[9].uLatitude,'f',7));

    ui->lineEdit_altitude_1->setText(QString::number(qRosNode.stQgcSaveMsg.stUavInfo[0].uAltitude,'f',7));
    ui->lineEdit_altitude_2->setText(QString::number(qRosNode.stQgcSaveMsg.stUavInfo[1].uAltitude,'f',7));
    ui->lineEdit_altitude_3->setText(QString::number(qRosNode.stQgcSaveMsg.stUavInfo[2].uAltitude,'f',7));
    ui->lineEdit_altitude_4->setText(QString::number(qRosNode.stQgcSaveMsg.stUavInfo[3].uAltitude,'f',7));
    ui->lineEdit_altitude_5->setText(QString::number(qRosNode.stQgcSaveMsg.stUavInfo[4].uAltitude,'f',7));
    ui->lineEdit_altitude_6->setText(QString::number(qRosNode.stQgcSaveMsg.stUavInfo[5].uAltitude,'f',7));
    ui->lineEdit_altitude_7->setText(QString::number(qRosNode.stQgcSaveMsg.stUavInfo[6].uAltitude,'f',7));
    ui->lineEdit_altitude_8->setText(QString::number(qRosNode.stQgcSaveMsg.stUavInfo[7].uAltitude,'f',7));
    ui->lineEdit_altitude_9->setText(QString::number(qRosNode.stQgcSaveMsg.stUavInfo[8].uAltitude,'f',7));
    ui->lineEdit_altitude_10->setText(QString::number(qRosNode.stQgcSaveMsg.stUavInfo[9].uAltitude,'f',7));

    ui->lineEdit_workState_1->setText(QString::number(qRosNode.stQgcSaveMsg.stUavInfo[0].uWorkState,'f',7));
    ui->lineEdit_workState_2->setText(QString::number(qRosNode.stQgcSaveMsg.stUavInfo[1].uWorkState,'f',7));
    ui->lineEdit_workState_3->setText(QString::number(qRosNode.stQgcSaveMsg.stUavInfo[2].uWorkState,'f',7));
    ui->lineEdit_workState_4->setText(QString::number(qRosNode.stQgcSaveMsg.stUavInfo[3].uWorkState,'f',7));
    ui->lineEdit_workState_5->setText(QString::number(qRosNode.stQgcSaveMsg.stUavInfo[4].uWorkState,'f',7));
    ui->lineEdit_workState_6->setText(QString::number(qRosNode.stQgcSaveMsg.stUavInfo[5].uWorkState,'f',7));
    ui->lineEdit_workState_7->setText(QString::number(qRosNode.stQgcSaveMsg.stUavInfo[6].uWorkState,'f',7));
    ui->lineEdit_workState_8->setText(QString::number(qRosNode.stQgcSaveMsg.stUavInfo[7].uWorkState,'f',7));
    ui->lineEdit_workState_9->setText(QString::number(qRosNode.stQgcSaveMsg.stUavInfo[8].uWorkState,'f',7));
    ui->lineEdit_workState_10->setText(QString::number(qRosNode.stQgcSaveMsg.stUavInfo[9].uWorkState,'f',7));
    /*ui->lineEdit_subNodeState_1->setText(QString::number(pstClusterCntxt->astSubNodeState[1].enClusterSubNodeState,'f',7));
    ui->lineEdit_subNodeState_2->setText(QString::number(pstClusterCntxt->astSubNodeState[2].enClusterSubNodeState,'f',7));
    ui->lineEdit_subNodeState_3->setText(QString::number(pstClusterCntxt->astSubNodeState[3].enClusterSubNodeState,'f',7));
    ui->lineEdit_subNodeState_4->setText(QString::number(pstClusterCntxt->astSubNodeState[4].enClusterSubNodeState,'f',7));
    ui->lineEdit_subNodeState_5->setText(QString::number(pstClusterCntxt->astSubNodeState[5].enClusterSubNodeState,'f',7));

    ui->lineEdit_missionState_1->setText(QString::number(pstClusterCntxt->astSubNodeState[1].enClusterMissionState,'f',7));
    ui->lineEdit_missionState_2->setText(QString::number(pstClusterCntxt->astSubNodeState[2].enClusterMissionState,'f',7));
    ui->lineEdit_missionState_3->setText(QString::number(pstClusterCntxt->astSubNodeState[3].enClusterMissionState,'f',7));
    ui->lineEdit_missionState_4->setText(QString::number(pstClusterCntxt->astSubNodeState[4].enClusterMissionState,'f',7));
    ui->lineEdit_missionState_5->setText(QString::number(pstClusterCntxt->astSubNodeState[5].enClusterMissionState,'f',7));*/


    qRosNode.s_bIsUiUpdateFinished = true;
}

void MainWindow::slotLogViewUpdate()
{
    ui->listView_log->scrollToBottom();
}

void MainWindow::on_action_takeoff_triggered()
{
    qRosNode.operatorMsg.uEncodeDirectField = 0;
    qRosNode.operatorMsg.stMsgHeader.enClusterCmd = CLUSTER_COMMAND_SUBNODE_START;
    Q_EMIT signalRosActionTrigger();
}

void MainWindow::on_action_land_triggered()
{
    qRosNode.operatorMsg.uEncodeDirectField = 0;
    qRosNode.operatorMsg.stMsgHeader.enClusterCmd = CLUSTER_COMMAND_SUBNODE_END;
    Q_EMIT signalRosActionTrigger();
}

void MainWindow::on_button_sendScanArea_clicked()
{
    //ui->groupBox_scanArea->hide();//ui->groupBox_pathPoint->hide();//ui->groupBox_pickPoint->hide();//ui->button_sendWayPoint->hide();//ui->groupBox_targetGps->show();
    qRosNode.operatorMsg.uEncodeDirectField = CLUSTER_MAINNODE_MSG_IE_VELOCITY | CLUSTER_MAINNODE_MSG_IE_FORMATIPN_TYPE;
    Cluster_Command astMapArray[5] = {CLUSTER_COMMAND_NODE_CREATE_CLOSE,CLUSTER_COMMAND_SUBNODE_START,
                                      CLUSTER_COMMAND_SUBNODE_END, CLUSTER_COMMAND_MISSION_START, CLUSTER_COMMAND_MISSION_END};

    //if(0 != ui->comboBox_ScanArea_Ind->currentIndex())
    {
        qRosNode.operatorMsg.uEncodeDirectField |= CLUSTER_MAINNODE_MSG_IE_AREA_INFO_GPS;
    }
    //else
    {
        qRosNode.operatorMsg.uEncodeDirectField |= CLUSTER_MAINNODE_MSG_IE_AREA_INFO;
    }

    qRosNode.operatorMsg.stMsgHeader.enClusterCmd = astMapArray[ui->comboBox_cmd->currentIndex()];

    qRosNode.operatorMsg.stScanArea.uPathNum = 4;
    qRosNode.operatorMsg.stScanArea.aPathPointBuf[0].aArray[0] = ui->lineEdit_101->text().toDouble();
    qRosNode.operatorMsg.stScanArea.aPathPointBuf[0].aArray[1] = ui->lineEdit_102->text().toDouble();
    qRosNode.operatorMsg.stScanArea.aPathPointBuf[0].aArray[2] = ui->lineEdit_103->text().toDouble();
    qRosNode.operatorMsg.stScanArea.aPathPointBuf[1].aArray[0] = ui->lineEdit_104->text().toDouble();
    qRosNode.operatorMsg.stScanArea.aPathPointBuf[1].aArray[1] = ui->lineEdit_105->text().toDouble();
    qRosNode.operatorMsg.stScanArea.aPathPointBuf[1].aArray[2] = ui->lineEdit_106->text().toDouble();
    qRosNode.operatorMsg.stScanArea.aPathPointBuf[2].aArray[0] = ui->lineEdit_107->text().toDouble();
    qRosNode.operatorMsg.stScanArea.aPathPointBuf[2].aArray[1] = ui->lineEdit_108->text().toDouble();
    qRosNode.operatorMsg.stScanArea.aPathPointBuf[2].aArray[2] = ui->lineEdit_109->text().toDouble();
    qRosNode.operatorMsg.stScanArea.aPathPointBuf[3].aArray[0] = ui->lineEdit_110->text().toDouble();
    qRosNode.operatorMsg.stScanArea.aPathPointBuf[3].aArray[1] = ui->lineEdit_111->text().toDouble();
    qRosNode.operatorMsg.stScanArea.aPathPointBuf[3].aArray[2] = ui->lineEdit_112->text().toDouble();

    qRosNode.operatorMsg.velocityLimit = ui->lineEdit_max_speed->text().toFloat();

    qRosNode.operatorMsg.enFormationType = (Cluster_Formation_Type)ui->comboBox_formation->currentIndex();

    Q_EMIT signalRosActionTrigger();
}

void MainWindow::on_button_sendPathPoint_clicked()
{
    qRosNode.operatorMsg.uEncodeDirectField = CLUSTER_MAINNODE_MSG_IE_VELOCITY | CLUSTER_MAINNODE_MSG_IE_FORMATIPN_TYPE;
    Cluster_Command astMapArray[5] = {CLUSTER_COMMAND_NODE_CREATE_CLOSE,CLUSTER_COMMAND_SUBNODE_START,
                                      CLUSTER_COMMAND_SUBNODE_END, CLUSTER_COMMAND_MISSION_START, CLUSTER_COMMAND_MISSION_END};

    //if(0 != ui->comboBox_PathPoint_Ind->currentIndex())
    {
        qRosNode.operatorMsg.uEncodeDirectField |= CLUSTER_MAINNODE_MSG_IE_PATH_POINT_GPS;
    }
    //else
    {
        qRosNode.operatorMsg.uEncodeDirectField |= CLUSTER_MAINNODE_MSG_IE_PATH_POINT;
    }

    qRosNode.operatorMsg.stMsgHeader.enClusterCmd = astMapArray[ui->comboBox_cmd->currentIndex()];

    qRosNode.operatorMsg.pastSubNodePathPoint[0] = (Cluster_Path_Buf *)fnCluster_buf_AllocBuf(sizeof(Cluster_Path_Buf));
    qRosNode.operatorMsg.pastSubNodePathPoint[0]->uPathNum = 4;
    qRosNode.operatorMsg.pastSubNodePathPoint[0]->aPathPointBuf[0].aArray[0] = ui->lineEdit_201->text().toDouble();
    qRosNode.operatorMsg.pastSubNodePathPoint[0]->aPathPointBuf[0].aArray[1] = ui->lineEdit_202->text().toDouble();
    qRosNode.operatorMsg.pastSubNodePathPoint[0]->aPathPointBuf[0].aArray[2] = ui->lineEdit_203->text().toDouble();
    qRosNode.operatorMsg.pastSubNodePathPoint[0]->aPathPointBuf[1].aArray[0] = ui->lineEdit_204->text().toDouble();
    qRosNode.operatorMsg.pastSubNodePathPoint[0]->aPathPointBuf[1].aArray[1] = ui->lineEdit_205->text().toDouble();
    qRosNode.operatorMsg.pastSubNodePathPoint[0]->aPathPointBuf[1].aArray[2] = ui->lineEdit_206->text().toDouble();
    qRosNode.operatorMsg.pastSubNodePathPoint[0]->aPathPointBuf[2].aArray[0] = ui->lineEdit_207->text().toDouble();
    qRosNode.operatorMsg.pastSubNodePathPoint[0]->aPathPointBuf[2].aArray[1] = ui->lineEdit_208->text().toDouble();
    qRosNode.operatorMsg.pastSubNodePathPoint[0]->aPathPointBuf[2].aArray[2] = ui->lineEdit_209->text().toDouble();
    qRosNode.operatorMsg.pastSubNodePathPoint[0]->aPathPointBuf[3].aArray[0] = ui->lineEdit_210->text().toDouble();
    qRosNode.operatorMsg.pastSubNodePathPoint[0]->aPathPointBuf[3].aArray[1] = ui->lineEdit_211->text().toDouble();
    qRosNode.operatorMsg.pastSubNodePathPoint[0]->aPathPointBuf[3].aArray[2] = ui->lineEdit_212->text().toDouble();

    qRosNode.operatorMsg.velocityLimit = ui->lineEdit_max_speed->text().toFloat();

    qRosNode.operatorMsg.enFormationType = (Cluster_Formation_Type)ui->comboBox_formation->currentIndex();

    Q_EMIT signalRosActionTrigger();
}

void MainWindow::on_button_sendPickupPoint_clicked()
{
    qRosNode.operatorMsg.uEncodeDirectField = CLUSTER_MAINNODE_MSG_IE_VELOCITY | CLUSTER_MAINNODE_MSG_IE_FORMATIPN_TYPE;
    Cluster_Command astMapArray[5] = {CLUSTER_COMMAND_NODE_CREATE_CLOSE,CLUSTER_COMMAND_SUBNODE_START,
                                      CLUSTER_COMMAND_SUBNODE_END, CLUSTER_COMMAND_MISSION_START, CLUSTER_COMMAND_MISSION_END};

    //if(0 != ui->comboBox_PickupPoint_Ind->currentIndex())
    {
        qRosNode.operatorMsg.uEncodeDirectField |= CLUSTER_MAINNODE_MSG_IE_PICKUP_POINT_GPS;
    }
    //else
    {
        qRosNode.operatorMsg.uEncodeDirectField |= CLUSTER_MAINNODE_MSG_IE_PICKUP_POINT;
    }

    qRosNode.operatorMsg.stMsgHeader.enClusterCmd = astMapArray[ui->comboBox_cmd->currentIndex()];

    qRosNode.operatorMsg.stPickupPoint.aArray[0] = ui->lineEdit_301->text().toDouble();
    qRosNode.operatorMsg.stPickupPoint.aArray[1] = ui->lineEdit_302->text().toDouble();
    qRosNode.operatorMsg.stPickupPoint.aArray[2] = ui->lineEdit_303->text().toDouble();

    qRosNode.operatorMsg.velocityLimit = ui->lineEdit_max_speed->text().toFloat();

    qRosNode.operatorMsg.enFormationType = (Cluster_Formation_Type)ui->comboBox_formation->currentIndex();

    Q_EMIT signalRosActionTrigger();
}

void MainWindow::on_button_sendCmd_clicked()
{
    Cluster_Qgc_Cmd_IE astMapArray[15] = {CLUSTER_QGC_CONFIG_TARGET_INFO,
                                          CLUSTER_QGC_CONFIG_FLIGHT_INFO,
                                          CLUSTER_QGC_CMD_SELF_CHECK,
                                          CLUSTER_QGC_CMD_ACTIVE_POWER,
                                          CLUSTER_QGC_CMD_TAKEOFF,
                                          CLUSTER_QGC_CMD_STANDBY,
                                          CLUSTER_QGC_CMD_RETURN_PICTURE,
                                          CLUSTER_QGC_CMD_POINT_WORK,
                                          CLUSTER_QGC_CMD_ADD_TARGET,
                                          CLUSTER_QGC_CMD_DELETE_TARGET,
                                          CLUSTER_QGC_CMD_USER_CONTROL,
                                          CLUSTER_QGC_CMD_LAND};

    QString astCmdStr[15] = {             "CLUSTER_QGC_CONFIG_TARGET_INFO",
                                          "CLUSTER_QGC_CONFIG_FLIGHT_INFO",
                                          "CLUSTER_QGC_CMD_SELF_CHECK",
                                          "CLUSTER_QGC_CMD_ACTIVE_POWER",
                                          "CLUSTER_QGC_CMD_TAKEOFF",
                                          "CLUSTER_QGC_CMD_STANDBY",
                                          "CLUSTER_QGC_CMD_RETURN_PICTURE",
                                          "CLUSTER_QGC_CMD_POINT_WORK",
                                          "CLUSTER_QGC_CMD_ADD_TARGET",
                                          "CLUSTER_QGC_CMD_DELETE_TARGET",
                                          "CLUSTER_QGC_CMD_USER_CONTROL",
                                          "CLUSTER_QGC_CMD_LAND"};

    cUint8 uCombBoxIndex = ui->comboBox_cmd->currentIndex();

    memset(&qRosNode.stQgcSendMsg, 0, sizeof(QGC_To_Cluster_All));

    qRosNode.stQgcSendMsg.stMsgHeader.uMsgID = astMapArray[uCombBoxIndex];

    RosNode::s_LogStr.append(PRINT_HEADER+QString("Send Cmd: ")+astCmdStr[uCombBoxIndex]+QString::asprintf("(%x)",qRosNode.stQgcSendMsg.stMsgHeader.uMsgID));

    qRosNode.stQgcSendMsg.stMsgHeader.uDestID = ui->lineEdit_uavid->text().toUInt();
    qRosNode.stQgcSendMsg.stMsgHeader.uSourceID = CLUSTER_QGC_ID;
    RosNode::s_LogStr.append(PRINT_HEADER+QString::asprintf("Select UavId[%x]",qRosNode.stQgcSendMsg.stMsgHeader.uDestID));

    qRosNode.stQgcSendMsg.stConfigTargetInfo.uTargetAreaNum = 0;
    qRosNode.stQgcSendMsg.stConfigTargetInfo.uDeveiceID = qRosNode.stQgcSendMsg.stMsgHeader.uDestID;
    if(0 != ui->lineEdit_301->text().toUInt())
    {
        qRosNode.stQgcSendMsg.stConfigTargetInfo.auTargetType[0] = ui->comboBox_formation_3->currentIndex();
        qRosNode.stQgcSendMsg.stConfigTargetInfo.auAssumeTargetNum[0] = ui->lineEdit_301->text().toUInt();
        qRosNode.stQgcSendMsg.stConfigTargetInfo.astTargetGPSArea[0][0].aArray[0] = ui->lineEdit_101->text().toDouble();
        qRosNode.stQgcSendMsg.stConfigTargetInfo.astTargetGPSArea[0][0].aArray[1] = ui->lineEdit_102->text().toDouble();
        qRosNode.stQgcSendMsg.stConfigTargetInfo.astTargetGPSArea[0][0].aArray[2] = ui->lineEdit_103->text().toDouble();
        qRosNode.stQgcSendMsg.stConfigTargetInfo.astTargetGPSArea[0][1].aArray[0] = ui->lineEdit_104->text().toDouble();
        qRosNode.stQgcSendMsg.stConfigTargetInfo.astTargetGPSArea[0][1].aArray[1] = ui->lineEdit_105->text().toDouble();
        qRosNode.stQgcSendMsg.stConfigTargetInfo.astTargetGPSArea[0][1].aArray[2] = ui->lineEdit_106->text().toDouble();
        qRosNode.stQgcSendMsg.stConfigTargetInfo.astTargetGPSArea[0][2].aArray[0] = ui->lineEdit_107->text().toDouble();
        qRosNode.stQgcSendMsg.stConfigTargetInfo.astTargetGPSArea[0][2].aArray[1] = ui->lineEdit_108->text().toDouble();
        qRosNode.stQgcSendMsg.stConfigTargetInfo.astTargetGPSArea[0][2].aArray[2] = ui->lineEdit_109->text().toDouble();
        qRosNode.stQgcSendMsg.stConfigTargetInfo.astTargetGPSArea[0][3].aArray[0] = ui->lineEdit_110->text().toDouble();
        qRosNode.stQgcSendMsg.stConfigTargetInfo.astTargetGPSArea[0][3].aArray[1] = ui->lineEdit_111->text().toDouble();
        qRosNode.stQgcSendMsg.stConfigTargetInfo.astTargetGPSArea[0][3].aArray[2] = ui->lineEdit_112->text().toDouble();
        qRosNode.stQgcSendMsg.stConfigTargetInfo.uTargetAreaNum++;
        if(CLUSTER_QGC_CONFIG_TARGET_INFO == qRosNode.stQgcSendMsg.stMsgHeader.uMsgID)
        {
            RosNode::s_LogStr.append(PRINT_HEADER+QString::asprintf("Target Type[%x]",ui->comboBox_formation_3->currentIndex()));
            RosNode::s_LogStr.append(NEWLINE+QString::asprintf("Assume Target Num[%x]",ui->lineEdit_301->text().toUInt()));
            RosNode::s_LogStr.append(NEWLINE+QString::asprintf("Point A[%lf-%lf-%lf]",ui->lineEdit_101->text().toDouble(),ui->lineEdit_102->text().toDouble(),ui->lineEdit_103->text().toDouble()));
            RosNode::s_LogStr.append(NEWLINE+QString::asprintf("Point B[%lf-%lf-%lf]",ui->lineEdit_104->text().toDouble(),ui->lineEdit_105->text().toDouble(),ui->lineEdit_106->text().toDouble()));
            RosNode::s_LogStr.append(NEWLINE+QString::asprintf("Point C[%lf-%lf-%lf]",ui->lineEdit_107->text().toDouble(),ui->lineEdit_108->text().toDouble(),ui->lineEdit_109->text().toDouble()));
            RosNode::s_LogStr.append(NEWLINE+QString::asprintf("Point D[%lf-%lf-%lf]",ui->lineEdit_110->text().toDouble(),ui->lineEdit_111->text().toDouble(),ui->lineEdit_112->text().toDouble()));
        }
    }

    if(0 != ui->lineEdit_302->text().toUInt())
    {
        qRosNode.stQgcSendMsg.stConfigTargetInfo.auTargetType[1] = ui->comboBox_formation_4->currentIndex();
        qRosNode.stQgcSendMsg.stConfigTargetInfo.auAssumeTargetNum[1] = ui->lineEdit_302->text().toUInt();
        qRosNode.stQgcSendMsg.stConfigTargetInfo.astTargetGPSArea[1][0].aArray[0] = ui->lineEdit_Area_125->text().toDouble();
        qRosNode.stQgcSendMsg.stConfigTargetInfo.astTargetGPSArea[1][0].aArray[1] = ui->lineEdit_Area_126->text().toDouble();
        qRosNode.stQgcSendMsg.stConfigTargetInfo.astTargetGPSArea[1][0].aArray[2] = ui->lineEdit_Area_127->text().toDouble();
        qRosNode.stQgcSendMsg.stConfigTargetInfo.astTargetGPSArea[1][1].aArray[0] = ui->lineEdit_Area_128->text().toDouble();
        qRosNode.stQgcSendMsg.stConfigTargetInfo.astTargetGPSArea[1][1].aArray[1] = ui->lineEdit_Area_129->text().toDouble();
        qRosNode.stQgcSendMsg.stConfigTargetInfo.astTargetGPSArea[1][1].aArray[2] = ui->lineEdit_Area_130->text().toDouble();
        qRosNode.stQgcSendMsg.stConfigTargetInfo.astTargetGPSArea[1][2].aArray[0] = ui->lineEdit_Area_131->text().toDouble();
        qRosNode.stQgcSendMsg.stConfigTargetInfo.astTargetGPSArea[1][2].aArray[1] = ui->lineEdit_Area_132->text().toDouble();
        qRosNode.stQgcSendMsg.stConfigTargetInfo.astTargetGPSArea[1][2].aArray[2] = ui->lineEdit_Area_133->text().toDouble();
        qRosNode.stQgcSendMsg.stConfigTargetInfo.astTargetGPSArea[1][3].aArray[0] = ui->lineEdit_Area_134->text().toDouble();
        qRosNode.stQgcSendMsg.stConfigTargetInfo.astTargetGPSArea[1][3].aArray[1] = ui->lineEdit_Area_135->text().toDouble();
        qRosNode.stQgcSendMsg.stConfigTargetInfo.astTargetGPSArea[1][3].aArray[2] = ui->lineEdit_Area_136->text().toDouble();
        qRosNode.stQgcSendMsg.stConfigTargetInfo.uTargetAreaNum++;
        if(CLUSTER_QGC_CONFIG_TARGET_INFO == qRosNode.stQgcSendMsg.stMsgHeader.uMsgID)
        {
            RosNode::s_LogStr.append(PRINT_HEADER+QString::asprintf("Target Type[%x]",ui->comboBox_formation_4->currentIndex()));
            RosNode::s_LogStr.append(NEWLINE+QString::asprintf("Assume Target Num[%x]",ui->lineEdit_302->text().toUInt()));
            RosNode::s_LogStr.append(NEWLINE+QString::asprintf("Point A[%lf-%lf-%lf]",ui->lineEdit_Area_125->text().toDouble(),ui->lineEdit_Area_126->text().toDouble(),ui->lineEdit_Area_127->text().toDouble()));
            RosNode::s_LogStr.append(NEWLINE+QString::asprintf("Point B[%lf-%lf-%lf]",ui->lineEdit_Area_128->text().toDouble(),ui->lineEdit_Area_129->text().toDouble(),ui->lineEdit_Area_130->text().toDouble()));
            RosNode::s_LogStr.append(NEWLINE+QString::asprintf("Point C[%lf-%lf-%lf]",ui->lineEdit_Area_131->text().toDouble(),ui->lineEdit_Area_132->text().toDouble(),ui->lineEdit_Area_133->text().toDouble()));
            RosNode::s_LogStr.append(NEWLINE+QString::asprintf("Point D[%lf-%lf-%lf]",ui->lineEdit_Area_134->text().toDouble(),ui->lineEdit_Area_135->text().toDouble(),ui->lineEdit_Area_136->text().toDouble()));
        }
    }

    if(0 != ui->lineEdit_303->text().toUInt())
    {
        qRosNode.stQgcSendMsg.stConfigTargetInfo.auTargetType[2] = ui->comboBox_formation_5->currentIndex();
        qRosNode.stQgcSendMsg.stConfigTargetInfo.auAssumeTargetNum[2] = ui->lineEdit_303->text().toUInt();
        qRosNode.stQgcSendMsg.stConfigTargetInfo.astTargetGPSArea[2][0].aArray[0] = ui->lineEdit_Area_150->text().toDouble();
        qRosNode.stQgcSendMsg.stConfigTargetInfo.astTargetGPSArea[2][0].aArray[1] = ui->lineEdit_Area_151->text().toDouble();
        qRosNode.stQgcSendMsg.stConfigTargetInfo.astTargetGPSArea[2][0].aArray[2] = ui->lineEdit_Area_152->text().toDouble();
        qRosNode.stQgcSendMsg.stConfigTargetInfo.astTargetGPSArea[2][1].aArray[0] = ui->lineEdit_Area_153->text().toDouble();
        qRosNode.stQgcSendMsg.stConfigTargetInfo.astTargetGPSArea[2][1].aArray[1] = ui->lineEdit_Area_154->text().toDouble();
        qRosNode.stQgcSendMsg.stConfigTargetInfo.astTargetGPSArea[2][1].aArray[2] = ui->lineEdit_Area_155->text().toDouble();
        qRosNode.stQgcSendMsg.stConfigTargetInfo.astTargetGPSArea[2][2].aArray[0] = ui->lineEdit_Area_156->text().toDouble();
        qRosNode.stQgcSendMsg.stConfigTargetInfo.astTargetGPSArea[2][2].aArray[1] = ui->lineEdit_Area_157->text().toDouble();
        qRosNode.stQgcSendMsg.stConfigTargetInfo.astTargetGPSArea[2][2].aArray[2] = ui->lineEdit_Area_158->text().toDouble();
        qRosNode.stQgcSendMsg.stConfigTargetInfo.astTargetGPSArea[2][3].aArray[0] = ui->lineEdit_Area_159->text().toDouble();
        qRosNode.stQgcSendMsg.stConfigTargetInfo.astTargetGPSArea[2][3].aArray[1] = ui->lineEdit_Area_160->text().toDouble();
        qRosNode.stQgcSendMsg.stConfigTargetInfo.astTargetGPSArea[2][3].aArray[2] = ui->lineEdit_Area_161->text().toDouble();
        qRosNode.stQgcSendMsg.stConfigTargetInfo.uTargetAreaNum++;
        if(CLUSTER_QGC_CONFIG_TARGET_INFO == qRosNode.stQgcSendMsg.stMsgHeader.uMsgID)
        {
            RosNode::s_LogStr.append(PRINT_HEADER+QString::asprintf("Target Type[%x]",ui->comboBox_formation_5->currentIndex()));
            RosNode::s_LogStr.append(NEWLINE+QString::asprintf("Assume Target Num[%x]",ui->lineEdit_303->text().toUInt()));
            RosNode::s_LogStr.append(NEWLINE+QString::asprintf("Point A[%lf-%lf-%lf]",ui->lineEdit_Area_150->text().toDouble(),ui->lineEdit_Area_151->text().toDouble(),ui->lineEdit_Area_152->text().toDouble()));
            RosNode::s_LogStr.append(NEWLINE+QString::asprintf("Point B[%lf-%lf-%lf]",ui->lineEdit_Area_153->text().toDouble(),ui->lineEdit_Area_154->text().toDouble(),ui->lineEdit_Area_155->text().toDouble()));
            RosNode::s_LogStr.append(NEWLINE+QString::asprintf("Point C[%lf-%lf-%lf]",ui->lineEdit_Area_156->text().toDouble(),ui->lineEdit_Area_157->text().toDouble(),ui->lineEdit_Area_158->text().toDouble()));
            RosNode::s_LogStr.append(NEWLINE+QString::asprintf("Point D[%lf-%lf-%lf]",ui->lineEdit_Area_159->text().toDouble(),ui->lineEdit_Area_160->text().toDouble(),ui->lineEdit_Area_161->text().toDouble()));
        }
    }

    if(CLUSTER_QGC_CONFIG_TARGET_INFO == qRosNode.stQgcSendMsg.stMsgHeader.uMsgID)
    {
        RosNode::s_LogStr.append(PRINT_HEADER+QString::asprintf("ScanArea Num[%x]",qRosNode.stQgcSendMsg.stConfigTargetInfo.uTargetAreaNum));
    }

    qRosNode.stQgcSendMsg.stConfigFlightInfo.uDeveiceID = qRosNode.stQgcSendMsg.stMsgHeader.uDestID;
    qRosNode.stQgcSendMsg.stConfigFlightInfo.uZDBType = ui->comboBox_formation_6->currentIndex();
    qRosNode.stQgcSendMsg.stConfigFlightInfo.uRefHeight = ui->lineEdit_max_height->text().toUInt();
    qRosNode.stQgcSendMsg.stConfigFlightInfo.uRefVelocity = ui->lineEdit_max_speed->text().toUInt();
    qRosNode.stQgcSendMsg.stConfigFlightInfo.stPickupArea.aArray[0] = ui->lineEdit_304->text().toDouble();
    qRosNode.stQgcSendMsg.stConfigFlightInfo.stPickupArea.aArray[1] = ui->lineEdit_305->text().toDouble();
    qRosNode.stQgcSendMsg.stConfigFlightInfo.stPickupArea.aArray[2] = ui->lineEdit_306->text().toDouble();
    qRosNode.stQgcSendMsg.stConfigFlightInfo.stInitialGpsPos.aArray[0] = ui->lineEdit_307->text().toDouble();
    qRosNode.stQgcSendMsg.stConfigFlightInfo.stInitialGpsPos.aArray[1] = ui->lineEdit_308->text().toDouble();
    qRosNode.stQgcSendMsg.stConfigFlightInfo.stInitialGpsPos.aArray[2] = ui->lineEdit_309->text().toDouble();
    qRosNode.stQgcSendMsg.stConfigFlightInfo.stInitialAttitude.aArray[0] = ui->lineEdit_310->text().toDouble();
    qRosNode.stQgcSendMsg.stConfigFlightInfo.stInitialAttitude.aArray[1] = ui->lineEdit_311->text().toDouble();
    qRosNode.stQgcSendMsg.stConfigFlightInfo.stInitialAttitude.aArray[2] = ui->lineEdit_312->text().toDouble();

    if(CLUSTER_QGC_CONFIG_FLIGHT_INFO == qRosNode.stQgcSendMsg.stMsgHeader.uMsgID)
    {
        RosNode::s_LogStr.append(PRINT_HEADER+QString::asprintf("ZDB Type[%x]",ui->comboBox_formation_6->currentIndex()));
        RosNode::s_LogStr.append(NEWLINE+QString::asprintf("uRefHeight[%x]",ui->lineEdit_max_height->text().toUInt()));
        RosNode::s_LogStr.append(NEWLINE+QString::asprintf("uRefVelocity[%x]",ui->lineEdit_max_speed->text().toUInt()));
        RosNode::s_LogStr.append(NEWLINE+QString::asprintf("stPickupArea[%lf-%lf-%lf]",ui->lineEdit_304->text().toDouble(),ui->lineEdit_305->text().toDouble(),ui->lineEdit_306->text().toDouble()));
        RosNode::s_LogStr.append(NEWLINE+QString::asprintf("stInitialGpsPos[%lf-%lf-%lf]",ui->lineEdit_307->text().toDouble(),ui->lineEdit_308->text().toDouble(),ui->lineEdit_309->text().toDouble()));
        RosNode::s_LogStr.append(NEWLINE+QString::asprintf("stInitialAttitude[%lf-%lf-%lf]",ui->lineEdit_310->text().toDouble(),ui->lineEdit_311->text().toDouble(),ui->lineEdit_312->text().toDouble()));
    }
    //qRosNode.stQgcSendMsg.stConfigTargetInfo.uTargetType = ui->lineEdit_max_speed->text().toFloat();



    //qRosNode.operatorMsg.enFormationType = (Cluster_Formation_Type)ui->comboBox_formation->currentIndex();

    Q_EMIT signalRosActionTrigger();
}

void MainWindow::on_button_sendAll_clicked()
{
    qRosNode.operatorMsg.uEncodeDirectField = CLUSTER_MAINNODE_MSG_IE_VELOCITY | CLUSTER_MAINNODE_MSG_IE_FORMATIPN_TYPE;
    Cluster_Command astMapArray[5] = {CLUSTER_COMMAND_NODE_CREATE_CLOSE,CLUSTER_COMMAND_SUBNODE_START,
                                      CLUSTER_COMMAND_SUBNODE_END, CLUSTER_COMMAND_MISSION_START, CLUSTER_COMMAND_MISSION_END};

    //if(0 != ui->comboBox_ScanArea_Ind->currentIndex())
    {
        qRosNode.operatorMsg.uEncodeDirectField |= CLUSTER_MAINNODE_MSG_IE_AREA_INFO_GPS;
    }
    //else
    {
        qRosNode.operatorMsg.uEncodeDirectField |= CLUSTER_MAINNODE_MSG_IE_AREA_INFO;
    }

    //if(0 != ui->comboBox_PathPoint_Ind->currentIndex())
    {
        qRosNode.operatorMsg.uEncodeDirectField |= CLUSTER_MAINNODE_MSG_IE_PATH_POINT_GPS;
    }
    //else
    {
        qRosNode.operatorMsg.uEncodeDirectField |= CLUSTER_MAINNODE_MSG_IE_PATH_POINT;
    }

    //if(0 != ui->comboBox_PickupPoint_Ind->currentIndex())
    {
        qRosNode.operatorMsg.uEncodeDirectField |= CLUSTER_MAINNODE_MSG_IE_PICKUP_POINT_GPS;
    }
    //else
    {
        qRosNode.operatorMsg.uEncodeDirectField |= CLUSTER_MAINNODE_MSG_IE_PICKUP_POINT;
    }

    qRosNode.operatorMsg.stMsgHeader.enClusterCmd = astMapArray[ui->comboBox_cmd->currentIndex()];

    qRosNode.operatorMsg.stScanArea.uPathNum = 4;
    qRosNode.operatorMsg.stScanArea.aPathPointBuf[0].aArray[0] = ui->lineEdit_101->text().toDouble();
    qRosNode.operatorMsg.stScanArea.aPathPointBuf[0].aArray[1] = ui->lineEdit_102->text().toDouble();
    qRosNode.operatorMsg.stScanArea.aPathPointBuf[0].aArray[2] = ui->lineEdit_103->text().toDouble();
    qRosNode.operatorMsg.stScanArea.aPathPointBuf[1].aArray[0] = ui->lineEdit_104->text().toDouble();
    qRosNode.operatorMsg.stScanArea.aPathPointBuf[1].aArray[1] = ui->lineEdit_105->text().toDouble();
    qRosNode.operatorMsg.stScanArea.aPathPointBuf[1].aArray[2] = ui->lineEdit_106->text().toDouble();
    qRosNode.operatorMsg.stScanArea.aPathPointBuf[2].aArray[0] = ui->lineEdit_107->text().toDouble();
    qRosNode.operatorMsg.stScanArea.aPathPointBuf[2].aArray[1] = ui->lineEdit_108->text().toDouble();
    qRosNode.operatorMsg.stScanArea.aPathPointBuf[2].aArray[2] = ui->lineEdit_109->text().toDouble();
    qRosNode.operatorMsg.stScanArea.aPathPointBuf[3].aArray[0] = ui->lineEdit_110->text().toDouble();
    qRosNode.operatorMsg.stScanArea.aPathPointBuf[3].aArray[1] = ui->lineEdit_111->text().toDouble();
    qRosNode.operatorMsg.stScanArea.aPathPointBuf[3].aArray[2] = ui->lineEdit_112->text().toDouble();

    qRosNode.operatorMsg.pastSubNodePathPoint[0] = (Cluster_Path_Buf *)fnCluster_buf_AllocBuf(sizeof(Cluster_Path_Buf));
    qRosNode.operatorMsg.pastSubNodePathPoint[0]->uPathNum = 4;
    qRosNode.operatorMsg.pastSubNodePathPoint[0]->aPathPointBuf[0].aArray[0] = ui->lineEdit_201->text().toDouble();
    qRosNode.operatorMsg.pastSubNodePathPoint[0]->aPathPointBuf[0].aArray[1] = ui->lineEdit_202->text().toDouble();
    qRosNode.operatorMsg.pastSubNodePathPoint[0]->aPathPointBuf[0].aArray[2] = ui->lineEdit_203->text().toDouble();
    qRosNode.operatorMsg.pastSubNodePathPoint[0]->aPathPointBuf[1].aArray[0] = ui->lineEdit_204->text().toDouble();
    qRosNode.operatorMsg.pastSubNodePathPoint[0]->aPathPointBuf[1].aArray[1] = ui->lineEdit_205->text().toDouble();
    qRosNode.operatorMsg.pastSubNodePathPoint[0]->aPathPointBuf[1].aArray[2] = ui->lineEdit_206->text().toDouble();
    qRosNode.operatorMsg.pastSubNodePathPoint[0]->aPathPointBuf[2].aArray[0] = ui->lineEdit_207->text().toDouble();
    qRosNode.operatorMsg.pastSubNodePathPoint[0]->aPathPointBuf[2].aArray[1] = ui->lineEdit_208->text().toDouble();
    qRosNode.operatorMsg.pastSubNodePathPoint[0]->aPathPointBuf[2].aArray[2] = ui->lineEdit_209->text().toDouble();
    qRosNode.operatorMsg.pastSubNodePathPoint[0]->aPathPointBuf[3].aArray[0] = ui->lineEdit_210->text().toDouble();
    qRosNode.operatorMsg.pastSubNodePathPoint[0]->aPathPointBuf[3].aArray[1] = ui->lineEdit_211->text().toDouble();
    qRosNode.operatorMsg.pastSubNodePathPoint[0]->aPathPointBuf[3].aArray[2] = ui->lineEdit_212->text().toDouble();

    qRosNode.operatorMsg.stPickupPoint.aArray[0] = ui->lineEdit_301->text().toDouble();
    qRosNode.operatorMsg.stPickupPoint.aArray[1] = ui->lineEdit_302->text().toDouble();
    qRosNode.operatorMsg.stPickupPoint.aArray[2] = ui->lineEdit_303->text().toDouble();

    qRosNode.operatorMsg.velocityLimit = ui->lineEdit_max_speed->text().toFloat();

    qRosNode.operatorMsg.enFormationType = (Cluster_Formation_Type)ui->comboBox_formation->currentIndex();

    Q_EMIT signalRosActionTrigger();
}

void MainWindow::on_checkBox_use_default_param_stateChanged(int arg1)
{
    if(arg1 == 0)
    {
        ui->lineEdit_master_url->setEnabled(true);
        ui->lineEdit_ros_ip->setEnabled(true);
    }
    else
    {
        ui->lineEdit_master_url->setEnabled(false);
        ui->lineEdit_ros_ip->setEnabled(false);
    }
}

void MainWindow::on_button_Throw_clicked()
{
    qRosNode.operatorMsg.uEncodeDirectField = 0;
    qRosNode.operatorMsg.stMsgHeader.enClusterCmd = CLUSTER_COMMAND_THROW_COMMAND;
    Q_EMIT signalRosActionTrigger();
}

void MainWindow::on_button_openView_clicked()
{
    qRosNode.operatorMsg.uEncodeDirectField = 0;
    if(bControlBtnState[3] == false)
    {
        //qRosNode.operatorMsg.stMsgHeader.enClusterCmd = CLUSTER_COMMAND_OPEN_VIEW;
        ui->button_openView->setText("Close View");
        bControlBtnState[3] = true;
    }
    else
    {
        //qRosNode.operatorMsg.stMsgHeader.enClusterCmd = CLUSTER_COMMAND_CLOSE_VIEW;
        ui->button_openView->setText("Open View");
        bControlBtnState[3] = false;
    }
    Q_EMIT signalRosActionTrigger();
}

void MainWindow::handleTimeOut()
{
    //qRosNode.interfaceMsg.uEncodeDirectField = UAV_MSG_IE_STRING;
//    qRosNode.interfaceMsg.enUavCmd = UAV_COMMAND_START_DETECT;
    //qRosNode.interfaceMsg.stringLen = dataTime.toString().toLatin1().size();
    //memcpy(qRosNode.interfaceMsg.stringMsg, dataTime.toString().toLatin1().data(),qRosNode.interfaceMsg.stringLen);
    /*//qDebug() << qRosNode.interfaceMsg.stringLen <<"\n\n\n";
    for(int i = 0; i < qRosNode.interfaceMsg.stringLen; i++)
    {
        qDebug() << qRosNode.interfaceMsg.stringMsg[i];
    }*/
    //Q_EMIT signalRosActionTrigger();
}

void MainWindow::on_button_startTime_clicked()
{
    timer.start(500);
}

void MainWindow::on_button_quit_clicked()
{
    close();
}

void MainWindow::closeEvent(QCloseEvent *event)
{
    WriteSettings();
    Q_EMIT signalRosShutdown();
    QMainWindow::closeEvent(event);
}

void MainWindow::ReadSettings()
{
    QSettings settings("./parameter.ini", QSettings::IniFormat);
    restoreGeometry(settings.value("geometry").toByteArray());
    restoreState(settings.value("windowState").toByteArray());

    QString master_url = settings.value("master_url",QString("http://192.168.1.2:11311/")).toString();
    QString host_url = settings.value("host_url", QString("192.168.1.3")).toString();

    ui->lineEdit_master_url->setText(master_url);
    ui->lineEdit_ros_ip->setText(host_url);

    bool checked = settings.value("use_environment_variables", false).toBool();
    ui->checkBox_use_default_param->setChecked(checked);

    if ( checked ) {
        ui->lineEdit_master_url->setEnabled(false);
        ui->lineEdit_ros_ip->setEnabled(false);
    }

    ui->lineEdit_101->setText(settings.value("lineEdit_101").toString());
    ui->lineEdit_102->setText(settings.value("lineEdit_102").toString());
    ui->lineEdit_103->setText(settings.value("lineEdit_103").toString());
    ui->lineEdit_104->setText(settings.value("lineEdit_104").toString());
    ui->lineEdit_105->setText(settings.value("lineEdit_105").toString());
    ui->lineEdit_106->setText(settings.value("lineEdit_106").toString());
    ui->lineEdit_107->setText(settings.value("lineEdit_107").toString());
    ui->lineEdit_108->setText(settings.value("lineEdit_108").toString());
    ui->lineEdit_109->setText(settings.value("lineEdit_109").toString());
    ui->lineEdit_110->setText(settings.value("lineEdit_110").toString());
    ui->lineEdit_111->setText(settings.value("lineEdit_111").toString());
    ui->lineEdit_112->setText(settings.value("lineEdit_112").toString());

    ui->lineEdit_201->setText(settings.value("lineEdit_201").toString());
    ui->lineEdit_202->setText(settings.value("lineEdit_202").toString());
    ui->lineEdit_203->setText(settings.value("lineEdit_203").toString());
    ui->lineEdit_204->setText(settings.value("lineEdit_204").toString());
    ui->lineEdit_205->setText(settings.value("lineEdit_205").toString());
    ui->lineEdit_206->setText(settings.value("lineEdit_206").toString());
    ui->lineEdit_207->setText(settings.value("lineEdit_207").toString());
    ui->lineEdit_208->setText(settings.value("lineEdit_208").toString());
    ui->lineEdit_209->setText(settings.value("lineEdit_209").toString());
    ui->lineEdit_210->setText(settings.value("lineEdit_210").toString());
    ui->lineEdit_211->setText(settings.value("lineEdit_211").toString());
    ui->lineEdit_212->setText(settings.value("lineEdit_212").toString());

    ui->lineEdit_301->setText(settings.value("lineEdit_301").toString());
    ui->lineEdit_302->setText(settings.value("lineEdit_302").toString());
    ui->lineEdit_303->setText(settings.value("lineEdit_303").toString());

    ui->lineEdit_max_speed->setText(settings.value("lineEdit_max_speed").toString());
    ui->lineEdit_max_height->setText(settings.value("lineEdit_max_height").toString());
    ui->comboBox_pointform->setCurrentIndex(settings.value("pointform").toInt());
    ui->comboBox_formation_3->setCurrentIndex(settings.value("targettype1").toInt());
    ui->comboBox_formation_4->setCurrentIndex(settings.value("targettype2").toInt());
    ui->comboBox_formation_5->setCurrentIndex(settings.value("targettype3").toInt());
    ui->comboBox_formation_6->setCurrentIndex(settings.value("uavtype").toInt());
    //ui->comboBox_PathPoint_Ind->setCurrentIndex(settings.value("PathPoint_Ind").toInt());
    //ui->comboBox_PickupPoint_Ind->setCurrentIndex(settings.value("PickupPoint_Ind").toInt());
}

void MainWindow::WriteSettings()
{
    QSettings settings("./parameter.ini", QSettings::IniFormat);

    settings.setValue("use_environment_variables",QVariant(ui->checkBox_use_default_param->isChecked()));
    settings.setValue("master_url",ui->lineEdit_master_url->text());
    settings.setValue("host_url",ui->lineEdit_ros_ip->text());

    settings.setValue("geometry", saveGeometry());
    settings.setValue("windowState", saveState());

    settings.setValue("lineEdit_101", ui->lineEdit_101->text());
    settings.setValue("lineEdit_102", ui->lineEdit_102->text());
    settings.setValue("lineEdit_103", ui->lineEdit_103->text());
    settings.setValue("lineEdit_104", ui->lineEdit_104->text());
    settings.setValue("lineEdit_105", ui->lineEdit_105->text());
    settings.setValue("lineEdit_106", ui->lineEdit_106->text());
    settings.setValue("lineEdit_107", ui->lineEdit_107->text());
    settings.setValue("lineEdit_108", ui->lineEdit_108->text());
    settings.setValue("lineEdit_109", ui->lineEdit_109->text());
    settings.setValue("lineEdit_110", ui->lineEdit_110->text());
    settings.setValue("lineEdit_111", ui->lineEdit_111->text());
    settings.setValue("lineEdit_112", ui->lineEdit_112->text());

    settings.setValue("lineEdit_125", ui->lineEdit_Area_125->text());
    settings.setValue("lineEdit_126", ui->lineEdit_Area_126->text());
    settings.setValue("lineEdit_127", ui->lineEdit_Area_127->text());
    settings.setValue("lineEdit_128", ui->lineEdit_Area_128->text());
    settings.setValue("lineEdit_129", ui->lineEdit_Area_129->text());
    settings.setValue("lineEdit_130", ui->lineEdit_Area_130->text());
    settings.setValue("lineEdit_131", ui->lineEdit_Area_131->text());
    settings.setValue("lineEdit_132", ui->lineEdit_Area_132->text());
    settings.setValue("lineEdit_133", ui->lineEdit_Area_133->text());
    settings.setValue("lineEdit_134", ui->lineEdit_Area_134->text());
    settings.setValue("lineEdit_135", ui->lineEdit_Area_135->text());
    settings.setValue("lineEdit_136", ui->lineEdit_Area_136->text());

    settings.setValue("lineEdit_201", ui->lineEdit_201->text());
    settings.setValue("lineEdit_202", ui->lineEdit_202->text());
    settings.setValue("lineEdit_203", ui->lineEdit_203->text());
    settings.setValue("lineEdit_204", ui->lineEdit_204->text());
    settings.setValue("lineEdit_205", ui->lineEdit_205->text());
    settings.setValue("lineEdit_206", ui->lineEdit_206->text());
    settings.setValue("lineEdit_207", ui->lineEdit_207->text());
    settings.setValue("lineEdit_208", ui->lineEdit_208->text());
    settings.setValue("lineEdit_209", ui->lineEdit_209->text());
    settings.setValue("lineEdit_210", ui->lineEdit_210->text());
    settings.setValue("lineEdit_211", ui->lineEdit_211->text());
    settings.setValue("lineEdit_212", ui->lineEdit_212->text());

    settings.setValue("lineEdit_301", ui->lineEdit_301->text());
    settings.setValue("lineEdit_302", ui->lineEdit_302->text());
    settings.setValue("lineEdit_303", ui->lineEdit_303->text());
    settings.setValue("lineEdit_304", ui->lineEdit_304->text());
    settings.setValue("lineEdit_305", ui->lineEdit_305->text());
    settings.setValue("lineEdit_306", ui->lineEdit_306->text());

    settings.setValue("lineEdit_max_speed",ui->lineEdit_max_speed->text());
    settings.setValue("lineEdit_max_height",ui->lineEdit_max_height->text());
    settings.setValue("pointform",ui->comboBox_pointform->currentIndex());
    settings.setValue("targettype1",ui->comboBox_formation_3->currentIndex());
    settings.setValue("targettype2",ui->comboBox_formation_4->currentIndex());
    settings.setValue("targettype3",ui->comboBox_formation_5->currentIndex());
    settings.setValue("uavtype",ui->comboBox_formation_6->currentIndex());
    //settings.setValue("PathPoint_Ind",ui->comboBox_PathPoint_Ind->currentIndex());
    //settings.setValue("PickupPoint_Ind",ui->comboBox_PickupPoint_Ind->currentIndex());
}
